#include "Graph.h"
#include "ObjUsefulFunctions.h"
#include <iostream>
#include "ReportError.h"
#include "Search.h"
#include "File.h"
#include <glut.h>

Graph::Graph()
{
	m_NextNodeID = 0;
}

void Graph::AddNode(GraphNode& NewNode)
{
	if(!IsNodePresent(NewNode.ReturnPos()))
	{
		NewNode.SetId(m_NextNodeID); //Set the current node's ID;
		m_Nodes.push_back(NewNode); //Put that node into our node vector
		EdgeList EmptyEdgeList; //create an empty list of edges for this node
		m_Edges.push_back(EmptyEdgeList); // push that empty list into the vector
		m_NextNodeID++; //increment the node id value.
	}
}

void Graph::RemoveNode(int Id)
{
	EdgeList::iterator deleteit;
	for(int i = 0; i < m_Edges.size(); i++)
	{
		if(i != Id) //serch all edges that don't belong to the node we want to remove
		{
			//search the other nodes edges for ones that point toward the node we want to remove
			deleteit = m_Edges[i].end();
			for(auto it = m_Edges[i].begin(); it != m_Edges[i].end(); ++it)
			{
				if(it->ReturnEndNodeID() == Id)
				{
					deleteit = it;
					break;
				}
			}
			if(deleteit != m_Edges[i].end())
			{
				m_Edges[i].erase(deleteit);
			}
		}

	}

	m_Edges.erase(std::find(m_Edges.begin(), m_Edges.end(), m_Edges[Id]));
	m_Nodes.erase(std::find(m_Nodes.begin(), m_Nodes.end(), m_Nodes[Id]));
	m_NextNodeID--;
}

bool Graph::Test()
{
	std::vector<int> testPath;
	Search s;
	for(int i = 0; i < m_Nodes.size(); i++)
	{
		for(int j = 0; j < m_Nodes.size(); j++)
		{
			testPath = 	s.BFS(*this, i, j);
			if(testPath.size() == 1)
			{
				std::cout << "Error, Cant' find path from " << i << " To " << j << "\n";
				return false;
			}
		}
	}
	return true;
}

bool Graph::IsEmpty()
{
	return m_Nodes.empty() && m_Edges.empty();
}

int Graph::GetNumEdges()
{
	int count = 0;

	for(int i = 0; i < m_Edges.size(); i++)
	{
		count += m_Edges[i].size();
	}

	return count;
}

int Graph::GetNumNodes()
{
	return m_Nodes.size();
}

void Graph::Draw()
{
	//Draw Nodes
	for(int i = 0; i < m_Nodes.size(); i++)
	{
		glPushMatrix();
			glTranslatef(m_Nodes[i].ReturnPos().m_x, m_Nodes[i].ReturnPos().m_y, m_Nodes[i].ReturnPos().m_z);
			glutSolidSphere(0.1, 16, 16);
		glPopMatrix();
	}

	//Draw Edges
	for (int i = 0; i < m_Edges.size(); i++) //For every Node
	{
		for(auto it = m_Edges[i].begin(); it != m_Edges[i].end(); ++it) //all the edges that connect that node
		{
			glPushMatrix();
				glBegin(GL_LINES);
				glLineWidth(3);
				//Start Node
				Vec3f Start = m_Nodes[it->ReturnStartNodeID()].ReturnPos();
				glVertex3f(Start.m_x, Start.m_y, Start.m_z);

				//End Node
				Vec3f End = m_Nodes[it->ReturnEndNodeID()].ReturnPos();
				glVertex3f(End.m_x, End.m_y, End.m_z);
				glEnd();
			glPopMatrix();
		} 
	}
}

void Graph::DrawWithPath(std::vector<int>& path)
{
	//Draw Nodes
	for(int i = 0; i < m_Nodes.size(); i++)
	{
			auto it = std::find(path.begin(), path.end(), m_Nodes[i].ReturnId());
			glPushMatrix();
			glTranslatef(m_Nodes[i].ReturnPos().m_x, m_Nodes[i].ReturnPos().m_y, m_Nodes[i].ReturnPos().m_z);
			if(it != path.end())
			{
				glColor3f(1, 0, 0);
			}
			glutSolidSphere(0.1, 16, 16);
			glPopMatrix();
			glColor3f(1, 1, 1);
	}

	//Draw Edges
	for (int i = 0; i < m_Edges.size(); i++) //For every Node
	{
		for(auto it = m_Edges[i].begin(); it != m_Edges[i].end(); ++it) //all the edges that connect that node
		{
			Vec3f Start = m_Nodes[it->ReturnStartNodeID()].ReturnPos();
			Vec3f End = m_Nodes[it->ReturnEndNodeID()].ReturnPos();
	
			glPushMatrix();
			glBegin(GL_LINES);
			glLineWidth(3);
			//Start Node
			glVertex3f(Start.m_x, Start.m_y, Start.m_z);
	
			//End Node
			glVertex3f(End.m_x, End.m_y, End.m_z);
			glEnd();
			glPopMatrix();
			glColor3f(1, 1, 1);
		} 
	}
}


EdgeList Graph::GetEdgeList(int id)
{
	return m_Edges[id];
}

void Graph::AddEdge(GraphEdge& newEdge)
{
	m_Edges[newEdge.ReturnStartNodeID()].push_back(newEdge);
	GraphEdge tempedge(newEdge.ReturnEndNodeID(), newEdge.ReturnStartNodeID(), newEdge.ReturnEdgeCost());
	m_Edges[newEdge.ReturnEndNodeID()].push_back(tempedge);
}

int Graph::ReturnIDofClosest(Vec3f& pos)
{
	float closest = 999999999.9f;
	int id = 0;
	for(int i = 0; i < m_Nodes.size(); i++)
	{
		float distance = VecLength(pos - m_Nodes[i].ReturnPos());
		if(distance < closest)
		{
			closest = distance;
			id =  m_Nodes[i].ReturnId();
		}
	}
	return id;
}

int Graph::ReturnIDofClosest(int id)
{
	float closest = 100.0f;
	int ResultId = 0;
	for(int i = 0; i < m_Nodes.size(); i++) //for all nodes
	{
		if(m_Nodes[i].ReturnId() != id) // not the node we are checking for
		{
			float distance = VecLength(m_Nodes[id].ReturnPos() - m_Nodes[i].ReturnPos());
			if(distance < closest)
			{
				closest = distance;
				ResultId =  m_Nodes[i].ReturnId();
			}
		}
	}
	return ResultId;
}

GraphNode Graph::GetNode(int id)
{
	return m_Nodes[id];
}

void Graph::LoadGraphFromFile(const std::string& filename)
{
	//Template for Texts
	//GraphNode X Y Z
	//GraphEdge Id1 Id2 Cost

	File GraphFile;

	if(!GraphFile.OpenRead(filename))
	{
		ReportError("Could Not Open File: " + filename);
		return;
	}

	std::string Line;
	while(GraphFile.GetString(&Line))
	{
		Strings str = Split(Line, ' ');
		if(str[0] == "GraphNode")
		{
			GraphNode TempGN(Vec3f(ToFloat(str[1]), ToFloat(str[2]), ToFloat(str[3])));
			AddNode(TempGN);
			continue;
		}
		if(str[0] == "GraphEdge")
		{
			if(ToInt(str[1]) > m_Nodes.size())
			{
				ReportError("Node " + str[1] + " Doesn't Exist");
				continue;
			}
			else
			{
				AddEdge(GraphEdge(ToInt(str[1]), ToInt(str[2]), ToFloat(str[3])));
			}
		}
	}
}

bool Graph::IsEdgePresent(GraphEdge& edge)
{
	for(int i = 0; i < m_Edges.size(); i++)
	{

	}
	return true;
}

bool Graph::IsNodePresent(Vec3f& pos)
{
	for(int i = 0; i < m_Nodes.size(); i++)
	{
		if(m_Nodes[i].ReturnPos() == pos)
		{
			return true;
		}
	}
	return false;
}

bool Graph::IsNodePresent(int id)
{
	return id < m_Nodes.size();
}